#include "yolo_detect_pkg/yolov10.hpp"
#include "yolo_detect_pkg/yolo_detect.hpp"
#include <atomic>

YoloDetect::YoloDetect():Node("yolo_detect_pkg_node"){
  
  // ----------------------------------------declare parameters from config file----------------------------------------
  RCLCPP_INFO(this->get_logger(),"[YoloDetect] Node started.");
  // 读取airsim参数
  this->declare_parameter<std::string>("subscribers.camera_reading.topic", "/camera/images");
  this->declare_parameter<int>("subscribers.camera_reading.queue_size", 10);

  this->declare_parameter<std::string>("subscribers.global_pose.names", "/global_position/global");

  this->declare_parameter<std::string>("publishers.detection_image.topic", "/default_topic");
  this->declare_parameter<int>("publishers.detection_image.queue_size", 10);
  this->declare_parameter<bool>("publishers.detection_image.latch", false);



  this->declare_parameter<std::string>("publishers.detection_box.topic", "/default_topic");
  this->declare_parameter<int>("publishers.detection_box.queue_size", 10);
  this->declare_parameter<bool>("publishers.detection_box.latch", false);
  
  this->declare_parameter<std::string>("publishers.detection_label.topic", "/result_label");
  // 读取yolo模型参数
  this->declare_parameter<std::string>("yolo_model.weight_file.name", "/home/nvidia/soft_simulate/src/yolo_byte_traker/source/best.onnx");
  this->declare_parameter<float>("yolo_model.threshold.value", 0.3);
  this->declare_parameter<float>("yolo_model.nmsthreshold.value", 0.3);
  this->declare_parameter<int>("yolo_model.topk.value", 10);
  this->declare_parameter<std::vector<std::string>>("yolo_model.detection_classes.names", {} );


  this->declare_parameter<std::string>("services.calldetectserver.names","drone_yolo_detect");

  this->declare_parameter<std::string>("subscribers.call_server.topic","/drone_yolo_detect_server_topic");

  
  this->declare_parameter<std::string>("parameters.namesspace", "drone_12" );
  this->declare_parameter<int>("parameters.vehicle_id", 1 );


  this->declare_parameter<std::string>("publishers.dingwei_publisher", "/default_topic");  

  this->declare_parameter<std::string>("subscribers.location_sub.topic", "/vision_pose/pose");

  this->declare_parameter<std::string>("subscribers.location_position.topic", "/local_position/pose");
  
  // ----------------------------------------reader parameters from config file----------------------------------------
  namespaces = this->get_parameter("parameters.namesspace").as_string();

  vehcile_id_ = this->get_parameter("parameters.vehicle_id").as_int();
  yoloConfig_.conf_threshold = 0.5;
  yoloConfig_.conf_threshold = this->get_parameter("yolo_model.threshold.value").as_double();

  yoloConfig_.model_path = "/home/zhao/project/catkin_ws2/src/yolo_detect_pkg/source/yolov10m.onnx";
  yoloConfig_.model_path = this->get_parameter("yolo_model.weight_file.name").as_string();

  yoloConfig_.topK = 10;
  yoloConfig_.topK = this->get_parameter("yolo_model.topk.value").as_int();

  yoloConfig_.class_names  = {          };
  yoloConfig_.class_names = this->get_parameter("yolo_model.detection_classes.names").as_string_array();
  
  yoloConfig_.nms_threshold = 0.5;
  yoloConfig_.nms_threshold = this->get_parameter("yolo_model.nmsthreshold.value").as_double();
  service_name = this->get_parameter("services.calldetectserver.names").as_string();
  service_name = namespaces + service_name;
  gloabal_position_topic = this->get_parameter("subscribers.global_pose.names").as_string();
  RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] readparamers");
  
  locationDetectionBoxTopicName = this->get_parameter("publishers.detection_label.topic").as_string();
  locationDetectionBoxTopicName = namespaces + locationDetectionBoxTopicName;
  if (yoloConfig_.class_names.empty()|| yoloConfig_.model_path.empty()||service_name.empty() || gloabal_position_topic.empty()) 
  {
    rclcpp::shutdown();
  }
  RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] init().");
  detectionImageTopicName = "/detection_image/result_image";
  int detectionImageQueueSize = 1;
  bool detectionImageLatch = true;

  detectionImageTopicName = this->get_parameter(
        "publishers.detection_image.topic"
        ).as_string();
  detectionImageTopicName = namespaces + detectionImageTopicName;


  detectionImageQueueSize = this->get_parameter(
        "publishers.detection_image.queue_size"
        ).as_int();
  detectionImageLatch = this->get_parameter(
        "publishers.detection_image.latch"
        ).as_bool();
  detectionBoxTopicName = "/detection_image/result_box";
  int detectionBoxQueueSize = 1;
  bool detectionBoxLatch = true;
  detectionBoxTopicName = this->get_parameter(
        "publishers.detection_box.topic"
        ).as_string();
  detectionBoxTopicName = namespaces + detectionBoxTopicName;
  RCLCPP_INFO(this->get_logger(),"detectionBoxTopicName value is %s",detectionBoxTopicName.c_str());
  
  detectionBoxQueueSize = this->get_parameter(
        "publishers.detection_box.queue_size"
        ).as_int();
  detectionBoxLatch = this->get_parameter(
        "publishers.detection_box.latch"
        ).as_bool();
  locaiton_sub_ = this->get_parameter("subscribers.location_sub.topic").as_string();
  locaiton_sub_ = namespaces + locaiton_sub_;

  dingwei_topic_name = this->get_parameter("publishers.dingwei_publisher").as_string();


  cameraTopicName = this->get_parameter("subscribers.camera_reading.topic").as_string();
  
  cameraQueueSize = this->get_parameter(
        "subscribers.camera_reading.queue_size"
        ).as_int();

  this->get_parameter("subscribers.call_server.topic", call_server_topic_name);
  call_server_topic_name = namespaces + call_server_topic_name;

  this->get_parameter("subscribers.location_position.topic", local_position_sub);
  local_position_sub = namespaces + local_position_sub;
}

YoloDetect::~YoloDetect() {
  RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] Node shutting down.");
}

bool YoloDetect::readParameters() {
  
}

void YoloDetect::init() {
  yolo.init(yoloConfig_);

  image_transport::ImageTransport it(shared_from_this());
  imageSubscriber_ = it.subscribe(cameraTopicName, 10, std::bind(&YoloDetect::cameraCallback, this, std::placeholders::_1));
  // imageSubscriber_ = it.subscribe(cameraTopicName, 10, std::bind(&YoloDetect::cameraCallback, this, std::placeholders::_1));
  // cameraSubscriber_ = this->create_subscription<sensor_msgs::msg::Image>(cameraTopicName,rclcpp::QoS(10).best_effort(),std::bind(&YoloDetect::cameraCallback, this, std::placeholders::_1));
  
  // 这里发布压缩图像
  #if 0
  detectionImagePublisher_ = this->create_publisher<sensor_msgs::msg::Image>(detectionImageTopicName, detectionImageQueueSize);
  #else
  detectionCompressedImagePublisher_ = this->create_publisher<sensor_msgs::msg::CompressedImage>(detectionImageTopicName, detectionImageQueueSize);
  #endif
  detectionBoxPublisher_ = this->create_publisher<yolo_detect_message::msg::DetectBoxs>(detectionBoxTopicName, detectionBoxQueueSize);
  callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
  rclcpp::QoS qos_profile(rclcpp::KeepLast(10)); 
  qos_profile.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); // 使用 BEST_EFFORT 模式 
  
  service_ = this->create_service<yolo_detect_message::srv::Service>(service_name, std::bind(&YoloDetect::service_callback,this,std::placeholders::_1,std::placeholders::_2),rmw_qos_profile_services_default,callback_group_);
  
  service_status_subscriber_ = this->create_subscription<yolo_detect_message::msg::CallServerStatus>(call_server_topic_name,rclcpp::QoS(20).best_effort(),std::bind(&YoloDetect::service_status_callback,this,std::placeholders::_1));
  gloabal_position_topic = namespaces + gloabal_position_topic;
  locationDetectionBoxPublisher_ = this->create_publisher<yolo_detect_message::msg::LocationDetectBoxs>(locationDetectionBoxTopicName, 10);
  global_subscription_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
            gloabal_position_topic, rclcpp::QoS(5).best_effort(),
            std::bind(&YoloDetect::uav_global_position_callback, this, std::placeholders::_1));
  dingwei_pub_ = this->create_publisher<client_interfaces::msg::ConfigTarget>(dingwei_topic_name, 10);
  pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
       locaiton_sub_, rclcpp::QoS(50).best_effort(), std::bind(&YoloDetect::pose_sub, this, std::placeholders::_1));
  
  local_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
       local_position_sub, rclcpp::QoS(50).best_effort(), std::bind(&YoloDetect::local_pose_sub, this, std::placeholders::_1));
  info_topic();
}

void YoloDetect::cameraCallback(const std::shared_ptr<const sensor_msgs::msg::Image> msg) 
{
  // 记录接收到USB图像的信息
  RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] USB image received.");
  // 检查服务状态，如果不为true，则直接返回
  if (service_status.load() == false)
  {
    RCLCPP_ERROR(this->get_logger(),"[YoloObjectDetector] service is not ready.");
    return;
  } 

  cv::Mat image;
  {
    // 加锁保护图像数据
    std::lock_guard<std::mutex> lock_image(image_mutex);
    // 将ROS图像消息转换为OpenCV图像
    image = cv_bridge::toCvCopy(msg, "bgr8")->image;
  }
  int height = image.rows;
  int width = image.cols;

  // 创建图像副本
  cv::Mat camImageCopy_ = image.clone();

  // 使用YOLO模型进行检测
  auto result = yolo.detect(camImageCopy_);

  // 初始化检测结果消息
  yolo_detect_message::msg::DetectBoxs detectBox;
  yolo_detect_message::msg::DetectBox detectBox_;
  yolo_detect_message::msg::LocationDetectBox locationDetectBox_;
  yolo_detect_message::msg::LocationDetectBoxs locationDetectBoxs_;
  tag_id++;
  // if (result.size() > 0)
  // {
  //   std::string image_name = image_path + std::to_string(tag_id) + ".png";
  //   cv::imwrite(image_name, image);
  // }
  // 遍历检测结果
  for (size_t i = 0; i < result.size(); i++)
  {
    // 设置检测框的类别标签
    detectBox_.class_label = result[i].lable_name;
    // 设置检测框的概率
    detectBox_.probability = result[i].score;
    // 设置检测框的左上角坐标
    detectBox_.xmin = result[i].x1;
    detectBox_.ymin = result[i].x2;
    // 设置检测框的宽度和高度
    detectBox_.width = result[i].width;
    detectBox_.heigh = result[i].height;
    // 设置检测框的类别ID
    detectBox_.id = result[i].label;
    // 将检测框添加到结果列表中
    detectBox.detect_boxes.push_back(detectBox_);
  }

  for (size_t i = 0; i < result.size(); i++)
  {
    locationDetectBox_.class_label = result[i].lable_name;
    // 设置检测框的概率
    locationDetectBox_.probability = result[i].score;
    // 设置检测框的左上角坐标
    locationDetectBox_.xmin = result[i].x1;
    locationDetectBox_.ymin = result[i].x2;
    // 设置检测框的宽度和高度
    locationDetectBox_.width = result[i].width;
    locationDetectBox_.heigh = result[i].height;
    // 设置检测框的类别ID
    locationDetectBox_.id = result[i].label;
    // 将检测框添加到结果列表中
    err_x = (locationDetectBox_.xmin + result[i].width) / 2 - width / 2;
    err_y = (locationDetectBox_.ymin + result[i].height) /2  -  height / 2;
    int max_err_x = 800;
    int max_err_y = 800;
    if (((err_x < max_err_x && err_x > -1* max_err_x))&&(err_y < max_err_y && err_y > -1* max_err_y)){
      locationDetectBox_.is_false = true;
      locationDetectBox_.longtitude = uav_gloabal_position.longitude;
      locationDetectBox_.latitude = uav_gloabal_position.latitude;
      locationDetectBox_.altitude = uav_gloabal_position.altitude;
    }else{
      locationDetectBox_.is_false = false;
      locationDetectBox_.longtitude = 0.0;
      locationDetectBox_.latitude = 0.0;
      locationDetectBox_.altitude = 0.0;
    }
    
    
    locationDetectBoxs_.detect_boxes.push_back(locationDetectBox_);
  }
  
  for (size_t i = 0; i < result.size(); i++)
  {
    if (result[i].label == 6)
    {
      client_interfaces::msg::ConfigTarget ConfigTarget_data;

      // UAV_position = uav_gloabal_position;

      tf2::Quaternion q(UAV_position.pose.orientation.x,
                      UAV_position.pose.orientation.y,
                      UAV_position.pose.orientation.z,
                      UAV_position.pose.orientation.w);
      

      tf2::Matrix3x3 m(q);
      double roll, pitch, yaw;
      m.getRPY(roll, pitch, yaw);


      tf2::Quaternion local_q(local_UAV_position.pose.orientation.x,
                      local_UAV_position.pose.orientation.y,
                      local_UAV_position.pose.orientation.z,
                      local_UAV_position.pose.orientation.w);
      

      tf2::Matrix3x3 local_m(local_q);
      double local_roll, local_pitch, local_yaw;
      local_m.getRPY(local_roll, local_pitch, local_yaw);

      ConfigTarget_data.body_translation_x = UAV_position.pose.position.x;
      ConfigTarget_data.body_translation_y = UAV_position.pose.position.y;
      ConfigTarget_data.body_translation_z = UAV_position.pose.position.z;
      ConfigTarget_data.body_rotation_roll = local_roll;
      ConfigTarget_data.body_rotation_pitch = local_pitch;
      ConfigTarget_data.body_rotation_yaw = local_yaw;
      ConfigTarget_data.target_id = 6;
      ConfigTarget_data.airplane_id = vehcile_id_;

      int target_x = (result[i].x1 + result[i].width / 2);
      int target_y = (result[i].x2 + result[i].height / 2);
    
      ConfigTarget_data.picture_location_u = target_x;
      ConfigTarget_data.picture_location_v = target_y;

      uint64_t time = this->get_clock()->now().seconds()*1000;//+this->get_clock()->now().nanoseconds()/1000000;

      // RCLCPP_INFO(this->get_logger(),"Detection seconds %lf, nanosends %lf, ms %ld",this->get_clock()->now().seconds(),this->get_clock()->now().nanoseconds(),time);
      
      ConfigTarget_data.time_measure = time;
      dingwei_pub_->publish(ConfigTarget_data);
    }
  }
  
  // 绘制检测框并生成结果图像
  cv::Mat image_result = yolo.YOLOv10_Draw_Line(result,camImageCopy_);

  // 调整图像大小
  // int new_width;
  // int new_height;
  // float rate = 0.2;
  // if(image.cols > 640 || image.rows > 1080)
  // {
  //   new_width = image.cols * rate;
  //   new_height = image.rows * rate;
  //   cv::resize(image, image_result, cv::Size(new_width, new_height));
  // }

  // 发布检测图像
  publishDetectionImage(image_result);
  locationDetectBoxs_.header.stamp = this->get_clock()->now();
  locationDetectionBoxPublisher_->publish(locationDetectBoxs_);
  // 发布检测结果
  detectBox.header.stamp = this->get_clock()->now();
  detectionBoxPublisher_->publish(detectBox);
  RCLCPP_INFO(this->get_logger(),"Detection result has been published.");
  return;
}


bool YoloDetect::publishDetectionImage(const cv::Mat& detectionImage) {
    if (detectionImage.empty()){
      return false;
    }
    
    // 设置最小发布间隔（例如100ms = 10Hz）
    static auto last_publish_time = std::chrono::steady_clock::now();
    auto now = std::chrono::steady_clock::now();
    // 如果距上次发布不足100ms，则跳过
    if (std::chrono::duration_cast<std::chrono::milliseconds>(now - last_publish_time).count() < 100) {
        return false;
    }
    last_publish_time = now;

    // if (detectionImagePublisher_.getNumSubscribers() < 1) return false;
    std_msgs::msg::Header header;
    header.stamp = this->now();
    header.frame_id = "camera_frame";
    #if 1
    // 发布压缩图像消息
    auto img_msg = cv_bridge::CvImage(header, "bgr8", detectionImage).toCompressedImageMsg();
    detectionCompressedImagePublisher_->publish(*img_msg);
    #else     
    auto img_msg = cv_bridge::CvImage(header, "bgr8", detectionImage).toImageMsg();
    detectionImagePublisher_->publish(*img_msg);
    #endif
    RCLCPP_DEBUG(this->get_logger(),"Detection image has been published.");
    return true;
}

void YoloDetect::service_callback(const yolo_detect_message::srv::Service::Request::SharedPtr req,
                        const yolo_detect_message::srv::Service::Response::SharedPtr res){
    bool reqs = req->status;
    if(reqs){
        RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] service callback.");
    }
    else{
        RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] service callback false.");

    }
    service_status.store(reqs);
    res->success = true;
    res->drone_statue = "yolo detect service_status is " + std::to_string(service_status);
}

void YoloDetect::service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg){
  bool reqs = msg->status;
    if(reqs != service_status)
    {
        std::lock_guard<std::mutex> lock_image(service_status_mutex);
        service_status = reqs;
    }
    RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] service status is %d.",service_status);
}

void YoloDetect::uav_global_position_callback(const sensor_msgs::msg::NavSatFix::SharedPtr  msg){
    uav_gloabal_position = *msg;
    RCLCPP_INFO(this->get_logger(),"gloabal receiev posisiton");
}

void YoloDetect::pose_sub(const geometry_msgs::msg::PoseStamped::SharedPtr msg){
    RCLCPP_INFO(this->get_logger(),"receiev posisiton");
    UAV_position = *msg;
    RCLCPP_INFO(this->get_logger(),"receiev posisiton end");
}
void YoloDetect::local_pose_sub(const geometry_msgs::msg::PoseStamped::SharedPtr msg){
    RCLCPP_INFO(this->get_logger(),"receievss posisiton");
    local_UAV_position = *msg;
    RCLCPP_INFO(this->get_logger(),"receievss posisiton end");
}
void YoloDetect::info_topic(){
  RCLCPP_INFO(this->get_logger(),"namespaces: %s",namespaces.c_str());
  RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] info topic. start.");
  RCLCPP_INFO(this->get_logger(), "vehicle id: %d", vehcile_id_);
  RCLCPP_INFO(this->get_logger(),"detectionImageTopicName: %s",detectionImageTopicName.c_str());
  RCLCPP_INFO(this->get_logger(),"detectionBoxTopicName: %s",detectionBoxTopicName.c_str());
  RCLCPP_INFO(this->get_logger(),"service_name: %s",service_name.c_str());
  RCLCPP_INFO(this->get_logger(),"call_server_topic_name: %s",call_server_topic_name.c_str());
  RCLCPP_INFO(this->get_logger(),"gloabal_position_topic: %s",gloabal_position_topic.c_str());
  RCLCPP_INFO(this->get_logger(),"locationDetectionBoxTopicName: %s",locationDetectionBoxTopicName.c_str());

  RCLCPP_INFO(this->get_logger(),"camera_reading: %s",cameraTopicName.c_str());
  RCLCPP_INFO(this->get_logger(),"yoloConfig_.model_path: %s",yoloConfig_.model_path.c_str());
  RCLCPP_INFO(this->get_logger(),"yoloConfig_.conf_threshold: %f",yoloConfig_.conf_threshold);
  RCLCPP_INFO(this->get_logger(),"location is: %s",locaiton_sub_.c_str());

  RCLCPP_INFO(this->get_logger(),"location is: %s",dingwei_topic_name.c_str());

  RCLCPP_INFO(this->get_logger(),"local_position_sub is: %s",local_position_sub.c_str());

  RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] info topic. edd.");
}